from piper_sdk import C_PiperInterface
import sys
# 测试代码
if __name__ == "__main__":
    can = sys.argv[1]
    print(can)
    piper = C_PiperInterface(can)
    piper.ConnectPort()
    piper.MotionCtrl_1(0x02,0,0)#恢复
    # piper.MotionCtrl_2(0, 0, 0, 0x00)#位置速度模式
    piper.ModeCtrl(0x01,0x01, 5, 0x00)
    piper.JointCtrl(0,0,-10,0,0,0)
    # piper.EndPoseCtrl()